/* 
 *  Incub - incubate your baby robot
 *  Copyright (C) 2008 Stanislaw Szymczyk
 *  e-mail: sszymczy@gmail.com   www: http://cogville.cog-lab.com
 *
 *  This file is part of Incub.
 *
 *  Incub is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  Incub is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with Incub.  If not, see <http://www.gnu.org/licenses/>.
 * 
 */
 
#include <cassert>
#include "Gate.h"
#include "CommunicableMotor.h"

using std::string;
using std::vector;
using std::pair;

Gate::Gate(World* world, VWorldOut *vWorldOut, VWorldIn *vWorldIn) {
	this->world = world;
	this->vWorldOut = vWorldOut;
	this->vWorldIn = vWorldIn;
}
	
void Gate::registerCallback(IClientCallback_ptr call) {
	Corba::registerCallback(call);
}

void Gate::unregisterCallback(IClientCallback_ptr call) {
	Corba::unregisterCallback(call);
}

VWorldOut* Gate::getVWorldOut() {
	CORBA::add_ref(vWorldOut);
	return vWorldOut;
}

void Gate::setVWorldOut(VWorldOut *vWorldOut) {
	this->vWorldOut = vWorldOut;
}

VWorldIn* Gate::getVWorldIn() {
	CORBA::add_ref(vWorldIn);
	return vWorldIn;
}

void Gate::setVWorldIn(VWorldIn *vWorldIn) {
	copyWorld(vWorldIn, this->world);
}

World* Gate::getWorld() {
	return world;
}

void Gate::setWorld(World *world) {
	this->world = world;
}

pair<string, int> parseNode(string& path) {
	size_t nextNodeIndex = path.find('/',1);
	string node;
	int index;
	if(nextNodeIndex == string::npos) {
		node = path.substr(1);
		path.erase(0);
	} else {
		node = path.substr(1, nextNodeIndex - 1);
		path.erase(0, nextNodeIndex);
	}
	size_t braceIndex = node.find('[',0);
	if(braceIndex == string::npos) {
		index = 0;
	} else {
		index = atoi(node.substr(braceIndex+1, node.size()-(braceIndex+2)).c_str()) - 1;
		node.erase(braceIndex);
	}
	
	return make_pair(node, index);
}

CORBA::ValueBase* parseEntityOut(VEntityOut *vEntity, string& path);

CORBA::ValueBase* parseJointOut(VJointOut *vJoint, string& path) {
	pair<string, int> node = parseNode(path);
	if(node.first == "Entity") {
		return parseEntityOut(vJoint->entity(), path);
	} else if(node.first == "Motor") {
		VMotorsOut* motors = vJoint->motors();
		return (*motors)[node.second];
	}
	assert(false);
	return NULL;
}

CORBA::ValueBase* parseEntityOut(VEntityOut *vEntity, string& path) {
	pair<string, int> node = parseNode(path);
	if(node.first == "Sensor") {
		VSensorsOut* sensors = vEntity->sensors();
		return (*sensors)[node.second];
	} else if(node.first == "Joint") {
		VJointsOut* joints = vEntity->joints();
		return parseJointOut((*joints)[node.second], path);
	}
	assert(false);
	return NULL;
}

CORBA::ValueBase* parseWorldOut(VWorldOut *vWorld, string& path) {
	pair<string, int> node;
	node = parseNode(path);
	node = parseNode(path);

	VEntitiesOut* entities = vWorld->entities();
	return parseEntityOut((*entities)[node.second], path);
}

template <class T>
T* parseEntityIn(Entity *entity, string& path);

template <class T>
T* parseJointIn(Joint *joint, string& path) {
	pair<string, int> node = parseNode(path);
	if(node.first == "Entity") {
		return parseEntityIn<T>(joint->getEntity(), path);
	}
	assert(false);
	return NULL;
}

template <>
Motor* parseJointIn(Joint *joint, string& path) {
	pair<string, int> node = parseNode(path);
	if(node.first == "Entity") {
		return parseEntityIn<Motor>(joint->getEntity(), path);
	} else if(node.first == "Motor") {
		vector<Motor*>& motors = joint->getMotors();
		return motors[node.second];
	}
	assert(false);
	return NULL;
}

template <class T>
T* parseEntityIn(Entity *entity, string& path) {
	pair<string, int> node = parseNode(path);
	if(node.first == "Joint") {
		vector<Joint*>& joints = entity->getJoints();
		return parseJointIn<T>(joints[node.second], path);
	}
	assert(false);
	return NULL;
}

template <>
Sensor* parseEntityIn(Entity *entity, string& path) {
	pair<string, int> node = parseNode(path);
	if(node.first == "Sensor") {
		vector<Sensor*>& sensors = entity->getSensors();
		return sensors[node.second];
	} else if(node.first == "Joint") {
		vector<Joint*>& joints = entity->getJoints();
		return parseJointIn<Sensor>(joints[node.second], path);
	}
	assert(false);
	return NULL;
}

template <class T>
T* parseWorldIn(World *world, string& path) {
	pair<string, int> node;
	node = parseNode(path);
	node = parseNode(path);

	vector<Entity*>& entities = world->getEntities();
	return parseEntityIn<T>(entities[node.second], path);
}

VSensorOut* Gate::getVSensorOut(const char* path) {
	string pathString(path);
	VSensorOut* sensor = dynamic_cast<VSensorOut*>(parseWorldOut(vWorldOut, pathString));
	CORBA::add_ref(sensor);
	return sensor;
}

void Gate::setVMotorIn(const char* path, VMotorIn* vMotor) {
	string pathString(path);
	Motor* motor = parseWorldIn<Motor>(this->world, pathString);
	dynamic_cast<GenericCommunicableMotor*>(motor)->copyMotor(vMotor);
}

VMotorOut* Gate::getVMotorOut(const char* path) {
	string pathString(path);
	VMotorOut* motor = dynamic_cast<VMotorOut*>(parseWorldOut(vWorldOut, pathString));
	CORBA::add_ref(motor);
	return motor;
}

void Gate::copyJoint(VJointIn *vJoint, Joint* joint) {
	copyEntity(vJoint->entity(), joint->getEntity());
	vector<Motor*>& motors = joint->getMotors();
	for(unsigned i = 0; i < motors.size(); i++) {
		Motor *motor = motors[i];
		dynamic_cast<GenericCommunicableMotor*>(motor)->copyMotor((*vJoint->motors())[i]);
	}
}

void Gate::copyEntity(VEntityIn *vEntity, Entity* entity) {
	vector<Joint*>& joints = entity->getJoints();
	for(unsigned i = 0; i != joints.size(); i++) copyJoint((*vEntity->joints())[i], joints[i]);
}

void Gate::copyWorld(VWorldIn *vWorld, World* world) {
	vector<Entity*>& entities = world->getEntities();
	for(unsigned i = 0; i < entities.size(); i++) copyEntity((*vWorld->entities())[i], entities[i]);
}
